import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    # 获取包的路径
    pkg_name = 'xunjian_robot'
    package_dir = get_package_share_directory(pkg_name)
    # 获取navigation2包的路径
    navigation2_dir = get_package_share_directory('navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建launch配置
    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='false')
    map_yaml_path = launch.substitutions.LaunchConfiguration('map', default=os.path.join(navigation2_dir, 'maps', 'new_map.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration('params_file', default=os.path.join(navigation2_dir, 'config', 'nav2_params.yaml'))
    waypoints_file = launch.substitutions.LaunchConfiguration('waypoints_file', default=os.path.join(package_dir, 'config', 'waypoints.yaml'))
    image_topic = launch.substitutions.LaunchConfiguration('image_topic', default='/camera/image_raw')

    # 语音节点可执行文件
    speaker_executable = 'speaker_node'
    # 巡检节点可执行文件
    patrol_executable = 'xunjian_robot'

    # 创建一个延迟启动的动作：在 rviz2 启动后 10 秒，启动 speaker 和 patrol 节点
    delayed_startup = launch.actions.TimerAction(
        period=10.0,  # 延迟 10 秒
        actions=[
            # 启动语音服务节点 (speaker_node)
            launch_ros.actions.Node(
                package=pkg_name,
                executable=speaker_executable,
                name='speaker_node',
                output='screen',
                parameters=[{'use_sim_time': use_sim_time}]
            ),
            # 启动巡检主节点 (xunjian_robot)
            launch_ros.actions.Node(
                package=pkg_name,
                executable=patrol_executable,
                name='xunjian_robot',
                output='screen',
                parameters=[waypoints_file, {'use_sim_time': use_sim_time}],
                remappings=[
                    ('/camera/image_raw', image_topic),
                ],
            ),
        ]
    )

    return launch.LaunchDescription([
        # 声明所有可被外部覆盖的参数
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time, description='Use simulation (Gazebo) clock if true.'),
        launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path, description='Full path to map file to load'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path, description='Full path to param file to load'),
        launch.actions.DeclareLaunchArgument('waypoints_file', default_value=waypoints_file, description='Full path to the waypoints YAML file.'),
        launch.actions.DeclareLaunchArgument('image_topic', default_value=image_topic, description='The image topic to subscribe to.'),

        # 1. 首先启动 navigation2 的 bringup (包含 map_server, lifecycle_manager, amcl, planner 等)
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py']
            ),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path
            }.items(),
        ),

        # 2. 启动 rviz2 可视化界面
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'
        ),

        # 3. 在 rviz2 启动 10 秒后，启动 speaker_node 和 xunjian_robot_node
        # 注意：这里的启动顺序不能乱，因为 speaker_node和 xunjian_robot_node 需要等待 rviz2 启动后才能正常工作
        delayed_startup
    ])